/*
 * @Author: xiaoxiaotaoya 1712331226@qq.com
 * @Date: 2024-12-18 21:21:29
 * @LastEditors: xiaoxiaotaoya 1712331226@qq.com
 * @LastEditTime: 2024-12-22 17:51:14
 * @FilePath: /trobot_ws/src/controller_modules/mc_path_planner/include/mc_path_planner/complete_coverage_path/ccpp_base.h
 * @Description: 这是默认设置,请设置`customMade`, 打开koroFileHeader查看配置 进行设置: https://github.com/OBKoro1/koro1FileHeader/wiki/%E9%85%8D%E7%BD%AE
 */
#ifndef __CCPP_BASE_H__
#define __CCPP_BASE_H__


#include <ros/ros.h>
#include <geometry_msgs/Point.h>
#include <geometry_msgs/Pose.h>
#include <geometry_msgs/Polygon.h>
#include <nav_msgs/Path.h>
#include <nav_msgs/OccupancyGrid.h>
#include <mc_core/utils/graphis_utils.h>
#include <mc_core/utils/math_utils.h>
#include <mc_core/msg_convert/msg_convert.h>
#include <mc_map/map_base.h>
// 路径平滑
#include <mc_path_planner/path_smoother/five_point_gradient.h>

using namespace std;

// https://guoyulong.work/archives/96
// https://blog.csdn.net/qq_44339029/article/details/136880603

namespace ccpp{

using Point2f = mc::Point2f;
using cv_points = vector<cv::Point>;
using gm_points = vector<geometry_msgs::Point>;

enum PathType{
    EDGE_PATH = 1,          //贴边路径
    BOW_SHAPED_PATH = 2,    //弓字形路径
    ARC_SHAPED_PATH = 3,    //圆弧形路径
    SPRIAL_AND_BOW_SHAPED_PATH = 4,        //螺旋弓字形路径
    SPRIAL_AND_ARC_SHAPED_PATH = 5         //螺旋圆弧形路径
};

struct CoveragePathConfig{
    double path_width = 0.3;    //路径宽度
};

// 多边形信息
struct PolygonInfo{
    gm_points origin_polygon;       //原始多边形信息
    cv_points polygon_pixels;       //多边形像素坐标
    geometry_msgs::Point origin_start;  //原始起点信息
    cv::Point start_pixels;          //起点像素坐标
};

// 轮廓信息
struct ContourInfo{
    // 轮廓提取数据
    cv_points contour;         //当前提取的轮廓
    double area;    //面积
    bool start_in_contour = false;
    pair<int, cv::Point> nearest_start_point;     //离起点最近的点索引和坐标
};


struct CoveragePath{
    vector<vector<Point2f>> coverage_path;  //全覆盖路径
    vector<vector<Point2f>> edge_path;      //贴边路径
};

// 全覆盖路径规划
class CoveragePlannerBase{
    public:
        CoveragePlannerBase(boost::shared_ptr<MapBase> map_handler_ptr, boost::shared_ptr<SmoothBase> smooth_ptr);
        ~CoveragePlannerBase();
        // 字符串转换
        string toString(PathType path_type);
        // 计算像素点距离
        double calculateDistance(cv::Point p1, cv::Point p2);
        // 计算距离Point2f
        double calculateDistance(const Point2f& p1, const Point2f& p2);
        // 根据起点和终点生成等间距的点
        std::vector<Point2f> generateLinePoints(const Point2f& start, const Point2f& end, double interval);
        // 稀疏点直接插值填充所有点
        std::vector<Point2f> fillAllPoints(vector<Point2f> points, double interval);
        // 坐标转换
        cv::Point Word2Opencv(geometry_msgs::Point point_2f);
        geometry_msgs::Point Opencv2World(cv::Point point);

    protected:
        cv::Mat originMap_;
        boost::shared_ptr<MapBase> mapHandlerPtr_;
        boost::shared_ptr<SmoothBase> smoothPathPtr_;

        CoveragePathConfig coveragePathConfig_;
        string debugDir_;
        bool debug_mode_;
        PolygonInfo polygonInfo_;
};

};  //namespace

#endif


 